from typing import List, Tuple
import numpy as np
from pyproj import Proj, Transformer


class TrajectoryGenerator:
    """
    根据GPS关键点生成连续轨迹，拐角处自动添加转弯圆弧。
    输出仅为[(lat, lon), ...]
    """

    def __init__(self):
        pass

    def _rotate90(self, v: np.ndarray) -> np.ndarray:
        return np.array([-v[1], v[0]])

    def _line_intersection(self, p1, d1, p2, d2):
        A = np.column_stack((d1, -d2))
        b = p2 - p1
        if abs(np.linalg.det(A)) < 1e-9:
            return None
        s_t = np.linalg.solve(A, b)
        return p1 + s_t[0] * d1

    def generate_fillet_path(
        self,
        gps_points: List[Tuple[float, float]],
        interval_m: float = 1.0,
        corner_radius_m: float = 5.0,
    ) -> List[Tuple[float, float]]:
        """
        生成带转弯半径的连续轨迹。

        参数:
            gps_points: [(lat, lon), ...] 至少两个点
            interval_m: 采样间隔（米）
            corner_radius_m: 拐角圆弧半径（米）
        返回:
            [(lat, lon), ...]
        """
        if len(gps_points) < 2:
            return gps_points

        # === 1. 局部投影（以首点为中心） ===
        lat0, lon0 = gps_points[0]
        proj = Proj(proj="aeqd", lat_0=lat0, lon_0=lon0, datum="WGS84", units="m")
        to_xy = Transformer.from_proj("epsg:4326", proj, always_xy=True)
        to_ll = Transformer.from_proj(proj, "epsg:4326", always_xy=True)

        pts_xy = [np.array(to_xy.transform(lon, lat)) for lat, lon in gps_points]
        pts_xy = np.array(pts_xy)
        n = len(pts_xy)

        # === 2. 计算路径（带圆滑拐角） ===
        traj_xy = [pts_xy[0].copy()]

        for i in range(1, n - 1):
            A, B, C = pts_xy[i - 1], pts_xy[i], pts_xy[i + 1]
            v1, v2 = (A - B), (C - B)
            l1, l2 = np.linalg.norm(v1), np.linalg.norm(v2)
            if l1 < 1e-6 or l2 < 1e-6:
                continue
            u1, u2 = v1 / l1, v2 / l2
            in_dir = -u1
            out_dir = u2
            cos_theta = np.clip(np.dot(in_dir, out_dir), -1.0, 1.0)
            theta = np.arccos(cos_theta)
            if abs(theta) < 1e-3:
                continue

            # 切线点间距
            t = corner_radius_m * np.tan(theta / 2.0)
            max_t = min(l1, l2) * 0.5
            if t > max_t:
                t = max_t
                r_eff = t / np.tan(theta / 2.0)
            else:
                r_eff = corner_radius_m

            T1 = B - in_dir * t
            T2 = B + out_dir * t

            # 找圆心
            dir_AB = (B - A) / np.linalg.norm(B - A)
            dir_BC = (C - B) / np.linalg.norm(C - B)
            perp1 = self._rotate90(dir_AB)
            perp2 = self._rotate90(dir_BC)

            center = None
            for s1 in [+1, -1]:
                for s2 in [+1, -1]:
                    cand = self._line_intersection(T1, s1 * perp1, T2, s2 * perp2)
                    if cand is None:
                        continue
                    if abs(np.linalg.norm(cand - T1) - np.linalg.norm(cand - T2)) < 1e-3:
                        center = cand
                        break
                if center is not None:
                    break
            if center is None:
                traj_xy.append(T1)
                traj_xy.append(T2)
                continue

            r = np.linalg.norm(center - T1)
            ang1 = np.arctan2(T1[1] - center[1], T1[0] - center[0])
            ang2 = np.arctan2(T2[1] - center[1], T2[0] - center[0])
            vA = T1 - center
            vB = T2 - center
            ccw = vA[0] * vB[1] - vA[1] * vB[0] > 0

            traj_xy.append(T1.copy())
            step = interval_m / max(1e-6, r)
            if ccw:
                if ang2 < ang1:
                    ang2 += 2 * np.pi
                angles = np.arange(ang1, ang2, step)
            else:
                if ang1 < ang2:
                    ang1 += 2 * np.pi
                angles = np.arange(ang1, ang2, -step)
            for ang in angles:
                x = center[0] + r * np.cos(ang)
                y = center[1] + r * np.sin(ang)
                traj_xy.append(np.array([x, y]))
            traj_xy.append(T2.copy())

        traj_xy.append(pts_xy[-1].copy())

        # === 3. 均匀重采样 ===
        traj_xy = np.array(traj_xy)
        deltas = np.diff(traj_xy, axis=0)
        seg_lens = np.linalg.norm(deltas, axis=1)
        cum = np.concatenate(([0.0], np.cumsum(seg_lens)))
        total_len = cum[-1]
        if total_len < 1e-6:
            lon, lat = to_ll.transform(traj_xy[0, 0], traj_xy[0, 1])
            return [(lat, lon)]
        n_samples = int(np.ceil(total_len / interval_m)) + 1
        s_vals = np.linspace(0, total_len, n_samples)
        resampled = []
        idx = 0
        for s in s_vals:
            while idx < len(cum) - 1 and cum[idx + 1] < s:
                idx += 1
            if idx >= len(cum) - 1:
                pt = traj_xy[-1]
            else:
                t = (s - cum[idx]) / (cum[idx + 1] - cum[idx])
                pt = traj_xy[idx] + t * (traj_xy[idx + 1] - traj_xy[idx])
            resampled.append(pt)
        resampled = np.array(resampled)

        # === 4. 转回经纬度 ===
        result = []
        for x, y in resampled:
            lon, lat = to_ll.transform(x, y)
            result.append((lat, lon))
        return result


# ========== 示例使用 ==========
if __name__ == "__main__":
    gps_points = [
        (37.7749, -122.4194),
        (37.7754, -122.4180),
        (37.7766, -122.4185),
        (37.7775, -122.4170),
    ]

    gen = TrajectoryGenerator()
    path = gen.generate_fillet_path(gps_points, interval_m=1.0, corner_radius_m=8.0)
    print(f"生成轨迹点数: {len(path)}")
    for p in path[:10]:
        print(p)
